#include "controler_task.h"

extern  Can_motor   m_0x201_can1;       //左前舵向6220
extern  Can_motor   m_0x202_can1;       //右前舵向6220
extern  Can_motor   m_0x203_can1;       //后舵向6220
extern  Can_motor   m_0x204_can1;       //左前轮向3508
extern  Can_motor   m_0x205_can1;       //右前轮向3508
extern  Can_motor   m_0x206_can1;       //后轮向3508

static Can_motor* motor_pointer[6];  //解耦指针数组
osThreadId_t	ContrlerTaskHandle;		    
osThreadAttr_t ContrlerTask_attributes;
remote_control_t remote_control;

extern "C" void ControlerTask(void *argument);
void remote_cmd_parser(remote_control_t* _remote_control);
void car_stop();
void car_run();

/*** 
 * @description: 设置控制器任务
 * @return {*}
 */
extern "C" void controler_task_init()
{

    ContrlerTask_attributes.name = "ContrlerTask";
    ContrlerTask_attributes.stack_size = 128 * 4;
    ContrlerTask_attributes.priority = (osPriority_t) osPriorityBelowNormal7;   
    ContrlerTaskHandle = osThreadNew(ControlerTask, NULL, &ContrlerTask_attributes);
}

/*** 
 * @description: 控制器任务函数
 * @param {void} *argument
 * @return {*}
 */
extern "C" void ControlerTask(void *argument)
{
    extern Uart uart6;

		bsp_uart_init();
		osDelay(200);	
		for(;;)
		{
			uart6.check_new_data(1); 	//等待新数据的到来
      uart6.acquire_data(&remote_control.Raw[0], uart6.acquire_data_length());
			remote_cmd_parser(&remote_control);
			osDelay(1);
		}
}

/*** 
 * @description: 遥控帧解析
 * @param {remote_control_t*} _remote_control
 * @return {*}
 */
void remote_cmd_parser(remote_control_t* _remote_control)
{   
		static int flag =0;
		static int bluetooth_flag=0;
		const int BLUETOOTH_CONNECTED = 0xaa;
		const int BLUETOOTH_DISCONNECTED = 0xbb;
	
    if (flag == 0)
    {
			flag = 1;
			
			//断联检测 - ESP32数据包第24字节(索引23)是连接状态
			_remote_control->connect_flag = _remote_control->Raw[23];
		if(_remote_control->connect_flag == 0xaa)  // 0xaa表示连接
		{
				bluetooth_flag = 1;
				car_run();
		}
		else        //其他全部都是断开
{
    car_stop();
    bluetooth_flag = 0;
}

			//处理底盘移动
#if 1
//处理底盘移动
// 左摇杆水平 -> X轴 (左右平移)
_remote_control->x = ((float)_remote_control->Raw[17] / 128.0f) * 10.0f - 10.0f;

// 左摇杆垂直 -> Y轴 (前后移动) - 注意ESP32已经对Y取反
_remote_control->y = ((float)_remote_control->Raw[18] / 128.0f) * 10.0f - 10.0f;

// 右摇杆水平 -> Z轴 (旋转)
_remote_control->z = ((float)_remote_control->Raw[19] / 128.0f) * 10.0f - 10.0f;

//遥杆死区控制
if (_remote_control->x < 1.5f && _remote_control->x > -1.5f) _remote_control->x = 0;
if (_remote_control->y < 1.5f && _remote_control->y > -1.5f) _remote_control->y = 0;
if (_remote_control->z < 1.5f && _remote_control->z > -1.5f) _remote_control->z = 0;
#endif

			//更新按键

			_remote_control->xbox_A 		= _remote_control->Raw[1];
			_remote_control->xbox_Y 		= _remote_control->Raw[2];
			_remote_control->xbox_B 		= _remote_control->Raw[3];
			_remote_control->xbox_X 		= _remote_control->Raw[4];
			_remote_control->xbox_Download 	= _remote_control->Raw[5];
			_remote_control->xbox_select 	= _remote_control->Raw[6];
			_remote_control->xbox_menu 		= _remote_control->Raw[7];
			_remote_control->xbox_logo 		= _remote_control->Raw[8];
			_remote_control->xbox_LB 		= _remote_control->Raw[9];
			_remote_control->xbox_LS 		= _remote_control->Raw[10];
			_remote_control->xbox_RB 		= _remote_control->Raw[11];
			_remote_control->xbox_RS 		= _remote_control->Raw[12];
			_remote_control->xbox_Left 		= _remote_control->Raw[13];
			_remote_control->xbox_Right 	= _remote_control->Raw[14];
			_remote_control->xbox_Up 		= _remote_control->Raw[15];
			_remote_control->xbox_Down 		= _remote_control->Raw[16];
			_remote_control->xbox_LT		= _remote_control->Raw[21];
			_remote_control->xbox_RT		= _remote_control->Raw[22];

			flag = 0;
		}
}

/*** 
 * @description: 车辆停止
 * @return {*}
 */
void car_stop()
{
	  m_0x201_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_ABS,
                                0x201);//左前舵
    m_0x202_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_ABS,
                                0x202);//右前舵
    m_0x203_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_ABS,
                                0x203);//后舵
    m_0x204_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x204);//左前轮
    m_0x205_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x205);//右前轮
    m_0x206_can1.measure_config(Can_motor::MOTOR_STATE::STOP,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x206);//后轮
}

/*** 
 * @description: 车辆运行
 * @return {*}
 */
void car_run()
{
	m_0x201_can1.measure_config(Can_motor::MOTOR_STATE::RUN,
                                Can_motor::MOTOR_STATE::ANGLE_ABS,
                                0x201);//左前舵
    m_0x202_can1.measure_config(Can_motor::MOTOR_STATE::RUN,
                                Can_motor::MOTOR_STATE::ANGLE_ABS,
                                0x202);//右前舵
    m_0x203_can1.measure_config(Can_motor::MOTOR_STATE::RUN,
                                Can_motor::MOTOR_STATE::ANGLE_ABS,
                                0x203);//后舵
    m_0x204_can1.measure_config(Can_motor::MOTOR_STATE::RUN,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x204);//左前轮
    m_0x205_can1.measure_config(Can_motor::MOTOR_STATE::RUN,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x205);//右前轮
    m_0x206_can1.measure_config(Can_motor::MOTOR_STATE::RUN,
                                Can_motor::MOTOR_STATE::ANGLE_NO,
                                0x206);//后轮
}